/*
 * DOpenGLPoseViewer.cpp
 *
 *  Created on: May 9, 2016
 *      Author: throne
 */

#include "DOpenGLPoseViewer.h"

DOpenGLPoseViewer::DOpenGLPoseViewer() {
	isInited = false;
	pointNum = 0;
	vao = 0;
	vbo_vertex = 0;
	vbo_norm = 0;
	vbo_color = 0;
	modelMatrix=glm::mat4(1.0f);

}

DOpenGLPoseViewer::~DOpenGLPoseViewer() {
	CleanUp();
}

bool DOpenGLPoseViewer::Init()
{
	glGenVertexArrays(1,&vao);
	if(vao == 0)goto InitFailed;
	glBindVertexArray(vao);

	glGenBuffers(1,&vbo_vertex);
	glGenBuffers(1,&vbo_norm);
	glGenBuffers(1,&vbo_color);

	if(vbo_vertex==0||vbo_norm==0 ||vbo_color==0)goto InitFailed;
	isInited = true;
	glBindBuffer(GL_ARRAY_BUFFER,0);
	glBindVertexArray(0);
	return true;
InitFailed:
	CleanUp();
	return false;
}
bool DOpenGLPoseViewer::IsInited() const
{
	return isInited;
}
void DOpenGLPoseViewer::CleanUp()
{
	if(vbo_vertex)glDeleteBuffers(1,&vbo_vertex);
	if(vbo_norm)glDeleteBuffers(1,&vbo_norm);
	if(vbo_color)glDeleteBuffers(1,&vbo_color);
	if(vao)glDeleteVertexArrays(1,&vao);
}

void DOpenGLPoseViewer::ResetModelMatrix()
{
	modelMatrix=glm::mat4(1.0f);
}

void DOpenGLPoseViewer::SetModelMatrix(const float pose_R[9], const float pose_T[3])
{
	modelMatrix=glm::mat4(pose_R[0],pose_R[1],pose_R[2],0,
		pose_R[3],pose_R[4],pose_R[5],0,
		pose_R[6],pose_R[7],pose_R[8],0,
		pose_T[0],pose_T[1],pose_T[2],1);
}
const float* DOpenGLPoseViewer::GetModelMatrixPtr() const
{
	return glm::value_ptr(modelMatrix);
}
bool DOpenGLPoseViewer::InitCamModel(float cam_xs , float cam_ys, float cam_zs )
{
	const int pointNum = 10*2;
	float cam_points[] = {
			0,0,0,
			-cam_xs,-cam_ys,cam_zs,
			0,0,0,
			-cam_xs,+cam_ys,cam_zs,
			0,0,0,
			+cam_xs,+cam_ys,cam_zs,
			0,0,0,
			+cam_xs,-cam_ys,cam_zs,

			+cam_xs,-cam_ys,cam_zs,
			+cam_xs,+cam_ys,cam_zs,
			+cam_xs,+cam_ys,cam_zs,
			-cam_xs,+cam_ys,cam_zs,
			-cam_xs,+cam_ys,cam_zs,
			-cam_xs,-cam_ys,cam_zs,
			-cam_xs,-cam_ys,cam_zs,
			+cam_xs,-cam_ys,cam_zs,

			0,0,0,
			cam_xs*2,0,0,
			0,0,0,
			0,cam_ys*2,0,
	};

	float point_color[pointNum*3];
	float point_norm[pointNum*3];
	for(int i=0;i<pointNum;i++)
	{
		point_color[i*3+0]=1.0f;
		point_color[i*3+1]=1.0f;
		point_color[i*3+2]=0.0f;
		point_norm[i*3+0]=std::numeric_limits<float>::quiet_NaN();
		point_norm[i*3+1]=std::numeric_limits<float>::quiet_NaN();
		point_norm[i*3+2]=std::numeric_limits<float>::quiet_NaN();
	}

	LoadLines(cam_points, 3, pointNum, GL_STATIC_DRAW);
	LoadColor(point_color, 3, pointNum, GL_STATIC_DRAW);
	LoadNorm(point_norm, 3, pointNum, GL_STATIC_DRAW);
}
bool DOpenGLPoseViewer::InitQuadcopterModel(float length = 0.6, float circle_radius = 0.1,
		float color_r = 1.0, float color_g = 0.1, float color_b = 0.1)
{
	const int CirclePointNum = 16;
	const int BodyPointNum = 4;
	const int TotalPointNum = 4*CirclePointNum + BodyPointNum;
	float cam_points[TotalPointNum*3];

	float half_length = length/2;
	cam_points[0] = half_length; cam_points[1] = half_length; cam_points[2] = 0;
	cam_points[3] = -half_length; cam_points[4] = -half_length; cam_points[5] = 0;
	cam_points[6] = half_length; cam_points[7] = half_length; cam_points[8] = 0;
	cam_points[9] = half_length; cam_points[10] = half_length; cam_points[11] = 0;

	for(int i=0;i<4;i++)
	{
		for(int j = 0; j<CirclePointNum;j++)
		{
			float *point_ptr = cam_points + 3*(BodyPointNum+CirclePointNum*i+j);
			point_ptr[0] = circle_radius*sin(j/CirclePointNum);
			point_ptr[1] = circle_radius*cos(j/CirclePointNum);
			point_ptr[2] = 0;
		}
	}

	float point_color[TotalPointNum*3];
	float point_norm[TotalPointNum*3];
	for(int i=0;i<TotalPointNum;i++)
	{
		point_color[i*3+0]=color_r;
		point_color[i*3+1]=color_g;
		point_color[i*3+2]=color_b;
		point_norm[i*3+0]=std::numeric_limits<float>::quiet_NaN();
		point_norm[i*3+1]=std::numeric_limits<float>::quiet_NaN();
		point_norm[i*3+2]=std::numeric_limits<float>::quiet_NaN();
	}

	LoadLines(cam_points, 3, TotalPointNum, GL_STATIC_DRAW);
	LoadColor(point_color, 3, TotalPointNum, GL_STATIC_DRAW);
	LoadNorm(point_norm, 3, TotalPointNum, GL_STATIC_DRAW);

	return true;
}
bool DOpenGLPoseViewer::LoadLines(const GLfloat* data, int float_per_point, int pointNum_, GLenum usage)
{
	if(!isInited) return false;
	int totalSize = pointNum_*float_per_point*sizeof(float);
	glBindVertexArray(vao);
	glBindBuffer(GL_ARRAY_BUFFER,vbo_vertex);
	glVertexAttribPointer(0, float_per_point, GL_FLOAT, GL_FALSE, 0, 0);
	glEnableVertexAttribArray(0);
	glBufferData(GL_ARRAY_BUFFER, totalSize,data,usage);

	glBindBuffer(GL_ARRAY_BUFFER,0);
	glBindVertexArray(0);
	pointNum = pointNum_;
	return true;


}
bool DOpenGLPoseViewer::LoadNorm(const GLfloat* data, int float_per_point, int pointNum_, GLenum usage)
{
	if(!isInited) return false;
	int totalSize = pointNum_*float_per_point*sizeof(float);
	glBindVertexArray(vao);
	glBindBuffer(GL_ARRAY_BUFFER,vbo_norm);
	glVertexAttribPointer(1, float_per_point, GL_FLOAT, GL_FALSE, 0, 0);
	glEnableVertexAttribArray(1);
	glBufferData(GL_ARRAY_BUFFER, totalSize,data,usage);

	glBindBuffer(GL_ARRAY_BUFFER,0);
	glBindVertexArray(0);
	pointNum = pointNum_;
	return true;
}
bool DOpenGLPoseViewer::LoadColor(const GLfloat* data, int float_per_point, int pointNum_, GLenum usage)
{
	if(!isInited) return false;
	int totalSize = pointNum_*float_per_point*sizeof(float);
	glBindVertexArray(vao);
	glBindBuffer(GL_ARRAY_BUFFER,vbo_color);
	glVertexAttribPointer(2, float_per_point, GL_FLOAT, GL_FALSE, 0, 0);
	glEnableVertexAttribArray(2);
	glBufferData(GL_ARRAY_BUFFER, totalSize,data,usage);

	glBindBuffer(GL_ARRAY_BUFFER,0);
	glBindVertexArray(0);
	pointNum = pointNum_;
	return true;
}


bool DOpenGLPoseViewer::DrawPoseAsCam(int render_modelMatrix_position) const
{
	if(!isInited) return false;
	glUniformMatrix4fv(render_modelMatrix_position, 1, GL_FALSE, GetModelMatrixPtr());
	glBindVertexArray(vao);
	glDrawArrays(GL_LINES, 0, pointNum);
	glBindVertexArray(0);
	return true;
}

GLuint DOpenGLPoseViewer::GetVAOHandle() const
{
	return vao;
}
GLuint DOpenGLPoseViewer::GetVBOHandle() const
{
	return vbo_vertex;
}
